#include <planner.h>

Planner::Planner(){
	zoom = 5.0;
	service = n.advertiseService("stalker_nav", &Planner::Stalk, this);
	sc = n.serviceClient<nav_msgs::GetMap>("/map_cells");
	cost_marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
}

bool Planner::Stalk(fnavfn::Stalk::Request  &req, fnavfn::Stalk::Response &res){
	/** Getting goal pose **/
	goalx = req.goalx;
	goaly = req.goaly;
	bool success = false;
	int attemptCount = 0;
	ControlClient cc;

	do{
		/** Getting Erratic's current pose **/
		getBaseFootprint();

		/** Getting full synced map **/
		queryMapServerForMap();

		/** Index the position in the matrices **/
		PII goal = index(goalx,goaly,resolution);
		PII bot = index(botx,boty,resolution);

		ROS_INFO("Goal set %lf %lf",goalx,goaly);
		ROS_INFO("Base Found %lf %lf %lf",botx,boty,bott);

		/** Calculating the obstacle cost **/
		obsc.calculate_obs_cost(grid);

		/** Calculating navigation function based on obstacle cost **/
		if( astar.computePath(grid,obsc.cost,bot,goal) ){
			intpltr.makeFiner(astar.navfn,fine,(int)zoom);

			ROS_INFO("Interpolation Complete");

			goal = index(goalx,goaly,resolution/zoom);
			bot = index(botx,boty,resolution/zoom);

			ROS_INFO("Calling gradient planner");
			path = gplanner.planPath(fine,bot,goal,(int)zoom);
			pathx.clear();
			pathy.clear();

			for(int i=0;i<path.size();i++){
				pathx.push_back(path[i].first * 0.02);
				pathy.push_back(path[i].second * 0.02);

				//std::cout << pathx[i] << " " << pathy[i] << std::endl;
			}

			ROS_INFO("First %d %d Last %d %d",path[0].first,path[0].second,path[path.size()-1].first,path[path.size()-1].second);

			CostMarker CM;
			//CM.Plot(astar.navfn,cost_marker_pub,0.1);
			for(int i=0;i<(int)path.size();i++)
				CM.PlotPoses(pathx[i]/0.02f,pathy[i]/0.02f,cost_marker_pub,0.02);

			ROS_INFO("Making request");


			success = cc.make_request(obsc.cost,astar.navfn,pathx,pathy,resolution,goalx,goaly);

			ROS_INFO("Returned from request %d",attemptCount);
			attemptCount++;
			if( attemptCount > 3 ){
				ROS_INFO("morethan three attempts");
				res.success = 0;
				return true;
			}
		}
	}while(!success);
	ROS_INFO("Normal exit");
	res.success = 1;
	return true;
}

void Planner::getBaseFootprint(){
	std::string source_frameid = "map";
	std::string target_frameid = "base_footprint";

	try{
		listener.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(10.0));
		listener.lookupTransform(source_frameid, target_frameid, ros::Time(), transform);
	}
	catch(tf::TransformException& ex)
	{
		std::cout << "Failure at "<< ros::Time::now() << std::endl;
		std::cout << "Exception thrown:" << ex.what()<< std::endl;
	}

	botx = transform.getOrigin().x();
	boty = transform.getOrigin().y();
	bott = get_yaw(transform);

	ROS_INFO("Initial position : %lf %lf %lf",botx,boty,bott);
}

void Planner::queryMapServerForMap(){
	grid.clear();
	nav_msgs::GetMap srv;
	if (sc.call(srv))	{
		width 		= srv.response.map.info.width;
		height 		= srv.response.map.info.height;
		resolution 	= srv.response.map.info.resolution;

		int cnt = 0;
		for(int i=0; i<height; i++){
			grid.push_back(VECTOR(width,0.0f));
			for( int j=0; j<width; j++){
				grid[i][j] = srv.response.map.data[cnt] == 1 ? INF : 0;
//				printf("%d",grid[i][j] == INF ? 1 : 0);
				cnt++;
//			}printf("\n");
			}
		}
	}else{
		ROS_ERROR("Map Server not set up");
	}
	ROS_INFO("GOT MAP >> %d %d %lf",width,height,resolution);
}
